#ifdef PCL
#include "ConvPCL.h"
#include "picojson.h"

/**
 * @brief 
 *      把img中的深度图转换为点云图
 * @param img 
 *      图像信息
 * @param intrinsics
 *      相机内参 
 * @return int 
 *    成功返回0，失败返回 !=0
 */
int ConverToPCL(STRC_IMG_ALL &img, char *intrinsics)
{
    int ret = 0;
    float fx = 1.0; // 焦距x
    float fy = 1.0; // 焦距y
    float cx = 0.0;  // 主点x
    float cy = 0.0; // 主点y
    int mImgWidth = 640 , mImgHeight = 480;
    uint16_t *pDepthData = img.img_depth.data;
    int mIndex = 0;
    if (img.img_depth.len == 0 || intrinsics == NULL)
    {
        return STATUS_INTRINSICS_ERROR;
    }
    picojson::value mIntrinsicsJson;
    picojson::object mObj;
    if (!picojson::parse(mIntrinsicsJson, std::string(intrinsics)).empty())
    {
        return STATUS_INTRINSICS_ERROR;
    }
    mObj = mIntrinsicsJson.get<picojson::object>();
    mImgWidth = atoi(mObj["width"].to_str().data());
    mImgHeight = atoi(mObj["height"].to_str().data());
    fx = atof(mObj["f_x"].to_str().data());
    fy = atof(mObj["f_y"].to_str().data());
    cx = atof(mObj["c_x"].to_str().data());
    cy = atof(mObj["c_y"].to_str().data());
    img.pCloud->is_dense = false;
    img.pCloud->width = mImgWidth;
    img.pCloud->height = mImgHeight;
    img.pCloud->points.resize(mImgWidth * mImgHeight);
    for (int i = 0; i < mImgHeight; i++)
    {
        for (int j = 0; j < mImgWidth; j++)
        {
            if (*pDepthData > 0 && *pDepthData < 0xFFF0)
            {
                img.pCloud->points[mIndex].x = (j - cx) * (*pDepthData) / fx;
                img.pCloud->points[mIndex].y = (i - cy) * (*pDepthData) / fy;
                img.pCloud->points[mIndex].z = *pDepthData;
            }else{
                img.pCloud->points[mIndex].x = std::numeric_limits<float>::quiet_NaN();
                img.pCloud->points[mIndex].y = std::numeric_limits<float>::quiet_NaN();
                img.pCloud->points[mIndex].z = std::numeric_limits<float>::quiet_NaN();
                
            }
            pDepthData++;
            mIndex++;
        }
    }
    return ret;
}

#endif // PCL 